/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package phys;

import spring.Element;
import vec.Vector3f;

/**
 *
 * @author Vic
 */
public class RK4 {

    static public void integrate(Element ele, float t, float dt) {
//        System.out.println(ele.getState());
        ele.getState().getVelocity().add(ele.getState().getImpulse().scale(ele.getState().getInverseMass()));
        ele.getState().getImpulse().clear();
        Derivative a = evaluate(ele, t, 0.0f, new Derivative());
        Derivative b = evaluate(ele, t, dt * 0.5f, a);
        Derivative c = evaluate(ele, t, dt * 0.5f, b);
        Derivative d = evaluate(ele, t, dt, c);

//          float dxdt = 1.0f/6.0f * (a.dx + 2.0f*(b.dx + c.dx) + d.dx);
        Vector3f dpdt = new Vector3f();
        dpdt.add(a.velocity);
        dpdt.add(b.velocity);
        dpdt.add(b.velocity);
        dpdt.add(c.velocity);
        dpdt.add(c.velocity);
        dpdt.add(d.velocity);
        dpdt.scale(dt / 6f);
        Vector3f dvdt = new Vector3f();
        dvdt.add(a.force);
        dvdt.add(b.force);
        dvdt.add(b.force);
        dvdt.add(c.force);
        dvdt.add(c.force);
        dvdt.add(d.force);
        dvdt.scale(dt / 6f);

        ele.getState().getPosition().add(dpdt);
        ele.getState().getVelocity().add(dvdt);
    }

    static public Derivative evaluate(Element el, float t, float dt, Derivative d) {
        State state = new State();
        state.setPosition( (Vector3f) (((Vector3f) d.velocity.clone()).scale(dt)).add(el.getState().position));
        state.setVelocity( (Vector3f) (((Vector3f) d.force.clone()).scale(dt)).add(el.getState().getVelocity()));

        Derivative output = new Derivative();
        output.velocity = (Vector3f) state.getVelocity().clone();
        output.force = el.getAcceleration(state, t + dt);
        return output;
    }
}
